#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <drive/BoundingBox.h>
#include <drive/BoundingBoxes.h>
#include "font/CvxText.h"
#include "string.h"
#include <queue>
#include <map>
using std::queue;
using std::pair;
using cv::Mat;
using std::cout;
using std::endl;

const int TEXT_POSITION_X = 5;
const int TEXT_POSITION_Y = 40;
const int TEXT_POSITION_Y_STEP = 40;
const double FONT_SCALE = 2.5;


ros::Time StartStamp;
bool GotResult = false;

drive::BoundingBoxes boundingBoxes;
#pragma region 渲染中文字体
CvxText text("/home/nanozhou/work/ROS/ros_vision/src/drive/src/res/MSYaHei.ttf"); //指定字体
cv::Scalar size1{40, 0.5, 0.1, 0}; // { 字体大小/空白比例/间隔比例/旋转角度 }
static int ToWchar(char *&src, wchar_t *&dest, const char *locale = "zh_CN.utf8")
{
    if (src == NULL)
    {
        dest = NULL;
        return 0;
    }

    // 根据环境变量设置locale
    setlocale(LC_CTYPE, locale);

    // 得到转化为需要的宽字符大小
    int w_size = mbstowcs(NULL, src, 0) + 1;

    // w_size = 0 说明mbstowcs返回值为-1。即在运行过程中遇到了非法字符(很有可能使locale没有设置正确)
    if (w_size == 0)
    {
        dest = NULL;
        return -1;
    }

    //wcout << "w_size" << w_size << endl;
    dest = new wchar_t[w_size];
    if (!dest)
    {
        return -1;
    }

    int ret = mbstowcs(dest, src, strlen(src) + 1);
    if (ret <= 0)
    {
        return -1;
    }
    return 0;
}

void drawChineseChars(cv::Mat &frame, char *str, int pos_x, int pos_y, cv::Scalar color)
{
    wchar_t *w_str;
    ToWchar(str, w_str);
    text.putText(frame, w_str, cv::Point(pos_x, pos_y), color);
}

void drawIniting(cv::Mat& canvas)
{
    char *str = (char *)"正在初始化...";
    std::string testStr(str);
    int font_face = cv::FONT_HERSHEY_COMPLEX;
    int baseline;
    cv::Size text_size = cv::getTextSize(testStr, font_face, FONT_SCALE, 2, &baseline);

    //将文本框居中绘制 
    int posX = canvas.cols / 2 - text_size.width / 2; 
    int posY = canvas.rows / 2 + text_size.height / 2;
    drawChineseChars(canvas, str, posX, posY, cv::Scalar(0, 0, 255)); //左上角绘制文字信息
}
#pragma region 根据msg信息,将信息绘制到图片上
void drawRunning(cv::Mat& canvas) {
    char *str = (char *) "";
    //文本直接显示在框的中心位置
  //  if(boundingBoxes.IsDistracted||boundingBoxes.IsDozeNod||boundingBoxes.IsEyeClosed||boundingBoxes.IsYawn||boundingBoxes.IsEyeOcclusion||boundingBoxes.IsMouthOcclusion||boundingBoxes.HeadCaptured||boundingBoxes.FaceCaptured||boundingBoxes.FaceLeftCaptured||boundingBoxes.FaceRightCaptured||boundingBoxes.FaceUpCaptured||boundingBoxes.FaceDownCaptured||boundingBoxes.HasCigarette||boundingBoxes.HasPhone)
  
             







    for (auto iter = boundingBoxes.boundingboxes.begin(); iter!=boundingBoxes.boundingboxes.end(); ++iter){
        if(iter->Class=="CIGARETTE")
        {
	    str=(char*)"检测到香烟！";
            cv::Rect r(iter->Rect_x,iter->Rect_y,iter->Rect_w,iter->Rect_h);
            cv::rectangle(canvas, r, cv::Scalar(0x27, 0xC1, 0x36), 2);
	    std::string testStr(str);
            int font_face = cv::FONT_HERSHEY_COMPLEX;
            int baseline;
            cv::Size text_size = cv::getTextSize(testStr, font_face, FONT_SCALE, 2, &baseline);

    //将文本框居中绘制 
            int posX = 30; 
            int posY =50;
            drawChineseChars(canvas, str, posX, posY, cv::Scalar(0, 0, 255)); //左上角绘制文字信息
        }
        if(iter->Class=="PHONE")
        {
	    str=(char*)"检测到手机！";
            cv::Rect r(iter->Rect_x,iter->Rect_y,iter->Rect_w,iter->Rect_h);
            cv::rectangle(canvas, r, cv::Scalar(0x27, 0xC1, 0x36), 2);
	    std::string testStr(str);
            int font_face = cv::FONT_HERSHEY_COMPLEX;
            int baseline;
            cv::Size text_size = cv::getTextSize(testStr, font_face, FONT_SCALE, 2, &baseline);

    //将文本框居中绘制 
            int posX = 30; 
            int posY = 70;
            drawChineseChars(canvas, str, posX, posY, cv::Scalar(0, 0, 255)); //左上角绘制文字信息
        }
        if(iter->Class=="FACE"||iter->Class=="FACE_LEFT"||iter->Class=="FACE_Right"||iter->Class=="FACE_UP"||iter->Class=="FACE_DOWN")
        {
            cv::Rect r(iter->Rect_x,iter->Rect_y,iter->Rect_w,iter->Rect_h);
	   

    //将文本框居中绘制 
         //   int posX = canvas.cols / 2 - text_size.width / 2; 
         //   int posY = canvas.rows / 2 + text_size.height / 2;
            
            if(boundingBoxes.IsDistracted||boundingBoxes.IsDozeNod||boundingBoxes.IsEyeOcclusion||boundingBoxes.IsMouthOcclusion||boundingBoxes.HeadCaptured||boundingBoxes.FaceCaptured||boundingBoxes.FaceLeftCaptured||boundingBoxes.FaceRightCaptured||boundingBoxes.FaceUpCaptured||boundingBoxes.FaceDownCaptured)
            {
                cv::rectangle(canvas, r, cv::Scalar(0x27, 0xC1, 0x36), 2);
            }
            if(boundingBoxes.FaceDownCaptured)
            {
                str=(char*)"face turndown!";//框绘制脸
	        std::string testStr(str);
                int font_face = cv::FONT_HERSHEY_COMPLEX;
                int baseline;
                cv::Size text_size = cv::getTextSize(testStr, font_face, FONT_SCALE, 2, &baseline);

    //将文本框居中绘制 
                int posX = 30; 
                int posY = 90;
                drawChineseChars(canvas, str, posX, posY, cv::Scalar(0, 0, 255)); //左上角绘制文字信息
               
            }
            if(boundingBoxes.FaceUpCaptured)
            {
                str=(char*)"face turnup!";//框绘制脸
	        std::string testStr(str);
                int font_face = cv::FONT_HERSHEY_COMPLEX;
                int baseline;
                cv::Size text_size = cv::getTextSize(testStr, font_face, FONT_SCALE, 2, &baseline);

    //将文本框居中绘制 
                int posX = 30; 
                int posY = 110;
                drawChineseChars(canvas, str, posX, posY, cv::Scalar(0, 0, 255)); //左上角绘制文字信息
               
            }
            if(boundingBoxes.FaceRightCaptured)
            {
                str=(char*)"face turnright!";//框绘制脸
	        std::string testStr(str);
                int font_face = cv::FONT_HERSHEY_COMPLEX;
                int baseline;
                cv::Size text_size = cv::getTextSize(testStr, font_face, FONT_SCALE, 2, &baseline);

    //将文本框居中绘制 
                int posX = 30; 
                int posY = 130;
                drawChineseChars(canvas, str, posX, posY, cv::Scalar(0, 0, 255)); //左上角绘制文字信息
               
            }
            if(boundingBoxes.FaceLeftCaptured)
            {
                str=(char*)"face turnleft!";//框绘制脸
	        std::string testStr(str);
                int font_face = cv::FONT_HERSHEY_COMPLEX;
                int baseline;
                cv::Size text_size = cv::getTextSize(testStr, font_face, FONT_SCALE, 2, &baseline);

    //将文本框居中绘制 
                int posX = 30; 
                int posY = 130;
                drawChineseChars(canvas, str, posX, posY, cv::Scalar(0, 0, 255)); //左上角绘制文字信息
               
            }
            if(boundingBoxes.FaceCaptured)
            {
                str=(char*)"catch face!";//框绘制脸
	        std::string testStr(str);
                int font_face = cv::FONT_HERSHEY_COMPLEX;
                int baseline;
                cv::Size text_size = cv::getTextSize(testStr, font_face, FONT_SCALE, 2, &baseline);

    //将文本框居中绘制 
                int posX = 30; 
                int posY = 150;
                drawChineseChars(canvas, str, posX, posY, cv::Scalar(0, 0, 255)); //左上角绘制文字信息
               
            }
            if(boundingBoxes.HeadCaptured)
            {
                //str=(char*)"catch head!";//框绘制脸
	       // std::string testStr(str);
                //int font_face = cv::FONT_HERSHEY_COMPLEX;
               // int baseline;
               // cv::Size text_size = cv::getTextSize(testStr, font_face, FONT_SCALE, 2, &baseline);

    //将文本框居中绘制 
               // int posX = 30; 
                //int posY = 130;
              //  drawChineseChars(canvas, str, posX, posY, cv::Scalar(0, 0, 255)); //左上角绘制文字信息
               
            }

            if(boundingBoxes.IsDistracted)
            {
                str=(char*)"分心中!";//框绘制脸
	        std::string testStr(str);
                int font_face = cv::FONT_HERSHEY_COMPLEX;
                int baseline;
                cv::Size text_size = cv::getTextSize(testStr, font_face, FONT_SCALE, 2, &baseline);


    //将文本框居中绘制 
                int posX = 30; 
                int posY = 170;
                drawChineseChars(canvas, str, posX, posY, cv::Scalar(0, 0, 255)); //左上角绘制文字信息
               
            }
            if(boundingBoxes.IsDozeNod)
            {
                str=(char*)"瞌睡中！";//绘制脸部
                std::string testStr(str);
                int font_face = cv::FONT_HERSHEY_COMPLEX;
                int baseline;
                cv::Size text_size = cv::getTextSize(testStr, font_face, FONT_SCALE, 2, &baseline);

    //将文本框居中绘制 
                int posX = 30; 
                int posY = 190;
                drawChineseChars(canvas, str, posX, posY, cv::Scalar(0, 0, 255)); //左上角绘制文字信息
            }


            if(boundingBoxes.IsEyeOcclusion)
            {
                str=(char*)"遮眼中！";
                std::string testStr(str);
                int font_face = cv::FONT_HERSHEY_COMPLEX;
                int baseline;
                cv::Size text_size = cv::getTextSize(testStr, font_face, FONT_SCALE, 2, &baseline);

    //将文本框居中绘制 
                int posX = 30; 
                int posY = 210;
                drawChineseChars(canvas, str, posX, posY, cv::Scalar(0, 0, 255)); //左上角绘制文字信息
            }
            if(boundingBoxes.IsMouthOcclusion)
            {
                str=(char*)"遮嘴中！";
                std::string testStr(str);
                int font_face = cv::FONT_HERSHEY_COMPLEX;
                int baseline;
                cv::Size text_size = cv::getTextSize(testStr, font_face, FONT_SCALE, 2, &baseline);

    //将文本框居中绘制 
                int posX =30; 
                int posY = 230;
                drawChineseChars(canvas, str, posX, posY, cv::Scalar(0, 0, 255)); //左上角绘制文字信息
            }
        }
        if(iter->Class=="YAWN"){
            cv::Rect r(iter->Rect_x,iter->Rect_y,iter->Rect_w,iter->Rect_h);
            cv::rectangle(canvas, r, cv::Scalar(0x27, 0xC1, 0x36), 2);
            str=(char*)"打哈欠中！";
            std::string testStr(str);
            int font_face = cv::FONT_HERSHEY_COMPLEX;
            int baseline;
            cv::Size text_size = cv::getTextSize(testStr, font_face, FONT_SCALE, 2, &baseline);

    //将文本框居中绘制 
            int posX =30; 
            int posY =250;
            drawChineseChars(canvas, str, posX, posY, cv::Scalar(0, 0, 255)); //左上角绘制文字信息
        }
        if((iter->Class=="LEFTEYE")&&boundingBoxes.IsEyeClosed){
            cv::Rect r(iter->Rect_x,iter->Rect_y,iter->Rect_w,iter->Rect_h);
            cv::rectangle(canvas, r, cv::Scalar(0x27, 0xC1, 0x36), 2);
            str=(char*)"闭眼驾驶中！";
            std::string testStr(str);
            int font_face = cv::FONT_HERSHEY_COMPLEX;
            int baseline;
            cv::Size text_size = cv::getTextSize(testStr, font_face, FONT_SCALE, 2, &baseline);

    //将文本框居中绘制 
            int posX = 30; 
            int posY = 270;
            drawChineseChars(canvas, str, posX, posY, cv::Scalar(0, 0, 255)); }//左上角绘制文字信息          
        if((iter->Class=="RIGHTEYE")&&boundingBoxes.IsEyeClosed){
            cv::Rect r(iter->Rect_x,iter->Rect_y,iter->Rect_w,iter->Rect_h);
            cv::rectangle(canvas, r, cv::Scalar(0x27, 0xC1, 0x36), 2);}}
            //str=(char*)"闭眼驾驶中！";
           // std::string testStr(str);
            //int font_face = cv::FONT_HERSHEY_COMPLEX;
            //int baseline;
           // cv::Size text_size = cv::getTextSize(testStr, font_face, FONT_SCALE, 2, &baseline);

    //将文本框居中绘制 
            //int posX = 10; 
            //int posY = 110;
           // drawChineseChars(canvas, str, posX, posY, cv::Scalar(0, 0, 255)); }//左上角绘制文字信息                 
}
#pragma endregion
#pragma endregion

ros::Time LatestResultStamp;
ros::Time CurAnalyzeStamp;
queue<pair<ros::Time, Mat>> FrameQueueDriver;

void ResultMsgInfoCallback(const drive::BoundingBoxes::ConstPtr& msg)
{
    cout << "单帧分析耗时： " << (int)((msg->CurAnalyzeStamp - msg->LatestResultStamp).toSec() * 1000) << endl;
    GotResult = true;
    
    LatestResultStamp = msg->LatestResultStamp;
    CurAnalyzeStamp = msg->CurAnalyzeStamp;
    boundingBoxes.boundingboxes.clear();
    for(auto iter=msg->boundingboxes.begin();iter!=msg->boundingboxes.end();++iter){
        boundingBoxes.boundingboxes.push_back(*iter);}

    //boundingBoxes.boundingboxes.assign(msg->boundingboxes.begin(),msg->boundingboxes.end());
    cout<<msg->HeadCaptured;
    boundingBoxes.HeadCaptured=msg->HeadCaptured;
    boundingBoxes.FaceCaptured=msg->FaceCaptured;
    boundingBoxes.FaceLeftCaptured=msg->FaceLeftCaptured;
    boundingBoxes.FaceRightCaptured=msg->FaceRightCaptured;
    boundingBoxes.FaceUpCaptured=msg->FaceUpCaptured;
    boundingBoxes.FaceDownCaptured=msg->FaceDownCaptured;
    boundingBoxes.IsDistracted=msg->IsDistracted;
    boundingBoxes.IsDozeNod=msg->IsDozeNod;
    boundingBoxes.IsEyeClosed=msg->IsEyeClosed;
    boundingBoxes.IsYawn=msg->IsYawn;
    boundingBoxes.IsEyeOcclusion=msg->IsEyeOcclusion;
    boundingBoxes.IsMouthOcclusion=msg->IsMouthOcclusion;
    boundingBoxes.HasCigarette=msg->HasCigarette;
    boundingBoxes.HasPhone=msg->HasPhone;
}

void DrawImageCallback(const sensor_msgs::ImageConstPtr& msg)
{
    try
    {
        ros::Time stamp = msg->header.stamp;
        Mat frame = cv_bridge::toCvShare(msg, "bgr8")->image;
        if(GotResult)
        {
            // 1.删除废帧
            while(FrameQueueDriver.size() > 0)
            {
                ros::Time tmp_stamp = FrameQueueDriver.front().first;
                if(tmp_stamp < LatestResultStamp)
                {
                    FrameQueueDriver.pop();
                }
                else
                {
                    break;
                }
            }
            cout << "剩余帧数：" << FrameQueueDriver.size() << endl;
            for(unsigned int i = 0; i < FrameQueueDriver.size(); i++)
            {
                cout << "I ";
            }
            cout << endl;

            // 2.符合条件的帧进入队列，队列长度控制在20以内
            if(FrameQueueDriver.size() < 20)
            {
                FrameQueueDriver.push(pair<ros::Time, Mat>(stamp, frame));
            }

            // 3.最老的一帧取出来渲染
            if(FrameQueueDriver.size() > 0 && FrameQueueDriver.front().first < CurAnalyzeStamp)
            {
                Mat canvas = FrameQueueDriver.front().second.clone();
                FrameQueueDriver.pop();
                drawRunning(canvas);
                cv::imshow("view", canvas);
            }
        }
        else
        {
            drawIniting(frame);
            cv::imshow("view", frame);
        } 
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
    }
}


int main(int argc, char **argv)
{
    ros::init(argc, argv, "image_render_node");
    ros::NodeHandle node_render;
#pragma region 初始化字体
    // 初始化字体
    int fontType;
    cv::Scalar fontSize;
    bool fontUnderline;
    float fontDiaphaneity;
    text.getFont(&fontType, &fontSize, &fontUnderline, &fontDiaphaneity);
    text.setFont(&fontType, &size1, &fontUnderline, &fontDiaphaneity);
#pragma endregion
    StartStamp = ros::Time::now();
    cv::namedWindow("view");
    cv::startWindowThread();

    ros::Subscriber person_info_sub = node_render.subscribe("/camera_csi0/cur_result", 1, ResultMsgInfoCallback);
    image_transport::ImageTransport it(node_render);
    image_transport::Subscriber sub = it.subscribe("/camera_csi0/frames", 1, DrawImageCallback);

    ros::spin();

    cv::destroyWindow("view");    //窗口

    return 0;
}
